function [ dVec, t, hFig ] = comparevector( vecCmp, tCmp, vecRef, tRef, varargin)
%COMPAREVECTOR 将多组3维向量数据与参考数据比较（如XYZ位置，速度，欧拉角等）
%
% Tips:
% # vecCmp/tCmp和vecRef/tRef的行数可以不同，本函数仅对tCmp和tRef中均存在的时刻的速度数据进行比较
%
% Input Arguments:
% # vecCmp: 长度为n的元胞数组，比较值，每个元素格式同vecRef
% # tCmp: 长度为n的元胞数组，比较值对应的时刻，每个元素格式同tRef
% # vecRef: 列数为3的数组，比较参考值，单位与vecCmp相同，为unit
% # tRef: 向量，速度比较参考值对应的时刻，其长度与vecRef行数相同，重复元素将被剔除，单位s
% # hFig: 外部给出的figure句柄，如为NaN则不绘图，如为空则新建绘图，否则在hFig指定的绘图对象上绘图，这时plotInOneFig选项无效（默认为true），默认为空
% # cmpName: 长度为n的元胞数组，每个元素为字符串，速度比较值的名称，用于绘图，默认为'比较速度i'
% # refName: 字符串，速度参考值的名称，用于绘图，默认为'参考速度'
% # figName: 字符串，绘图名称，默认为'速度比较'
% # orientSpec: 1*3的元胞数组，速度数组各列的方向标识，默认为东向、北向、天向
% # plotInOneFig: 逻辑标量，true表示各组比较结果绘制在一个图形上，否则各组比较结果分别绘图，默认为true
% # unit: 字符串，数据单位，默认为'm/s'
% # tol: 认为tCmp与tRef元素相等的阈值
% # colorSpec: 元胞向量，各曲线的颜色
%
% Output Arguments:
% # dVec: 尺寸与vecCmp相同的元胞数组，每个元素为列数为3的数组，第1-3列分别为在比较时刻的速度差值（比较值相对于参考值），单位为m/s
% # t: 尺寸与tCmp相同的元胞数组，每个元素为对应dVec元素的比较时刻，单位s
% # hFig: 图形句柄
%
% Assumptions and Limitations:
% # 图例仅支持前20组（包含参考值）数据

% NOTE: 本函数使用线性索引
n = length(vecCmp);

p = inputParser;
% 必须输入参数
addRequired(p, 'vecCmp', @iscell);
addRequired(p, 'tCmp', @iscell);
addRequired(p, 'vecRef', @isnumeric);
addRequired(p, 'tRef', @isnumeric);
% 可选输入参数
defaultHFig = [];
defaultCmpName = cell(size(vecCmp));
for i=1:n
    defaultCmpName{i} = ['比较速度' int2str(i)];
end
defaultRefName = '参考速度';
defaultFigName = '速度比较';
defaultOrientSpec = {'东向速度误差', '北向速度误差', '天向速度误差'};
defaultPlotInOneFig = true;
defaultTol = 1e-7;
defaultColorSpecs = {'-bd', 'g', 'r', 'c', 'm', 'y'};
defaultUnit = 'm/s';
addParameter(p, 'hFig', defaultHFig, @isnumeric);
addParameter(p, 'cmpName', defaultCmpName, @iscell);
addParameter(p, 'refName', defaultRefName, @ischar);
addParameter(p, 'figName', defaultFigName, @ischar);
addParameter(p, 'orientSpec', defaultOrientSpec, @iscell);
addParameter(p, 'plotInOneFig', defaultPlotInOneFig, @islogical);
addParameter(p, 'tol', defaultTol, @isnumeric);
addParameter(p, 'unit', defaultUnit, @ischar);
addParameter(p, 'colorSpecs', defaultColorSpecs, @iscell);
parse(p, vecCmp, tCmp, vecRef, tRef, varargin{:});

dVec = cell(size(vecCmp));
t = cell(size(tCmp));
[tRef, vecRef] = makeunique(tRef, vecRef);
for i=1:n
    dVec{i} = NaN(size(vecCmp{i}));
    t{i} = NaN(size(tCmp{i}));
    [tCmp{i}, vecCmp{i}] = makeunique(tCmp{i}, vecCmp{i});
    [indCmp, indRef, tmpt] = findeqelem(tCmp{i}, tRef, p.Results.tol);
    t{i}(indCmp, :) = tmpt;
    dVec{i}(indCmp, :) = vecCmp{i}(indCmp, :) - vecRef(indRef, :);
end

if isempty(p.Results.hFig) || ~isnan(p.Results.hFig)
    if ~isempty(p.Results.hFig)
        plotInOneFig = true;
    else
        plotInOneFig = p.Results.plotInOneFig;
    end
    for i=1:n
        if (i==1) || (~plotInOneFig)
            hFig = preparefig(p.Results.figName, p.Results.hFig);
        end
        
        for j=1:3
            if j == 1
                hMainAxis = subplot(3, 1, j);
            else
                subplot(3, 1, j);
            end
            if (i==1) || (~plotInOneFig)
                set(gca, 'NextPlot', 'add');
                xlabel('时间(s)');
                title([p.Results.orientSpec{j} '(' p.Results.unit ')（相对于' p.Results.refName '）']);
                grid on;
            end
            plot(tCmp{i}, dVec{i}(:, j), p.Results.colorSpecs{mod(i-1, length(p.Results.colorSpecs))+1}, 'DisplayName', p.Results.cmpName{i}, 'LineWidth', 1.0);
        end
        
        if (i==n) || (~plotInOneFig)
            legend(hMainAxis, 'show', 'Location', 'NorthWest');
        end
    end
end
end

function [t, vec] = makeunique(t, vec)
[tUniq, ia] = unique(t);
if ~isequal(t, tUniq)
    t = tUniq;
    vec = vec(ia, :);
    warning('comparevec:notsortedorunique', 'Time vector(s) is not sorted or contains repeated element(s).');
end
end